#include "jkservo.h"
#include "rtbus/rtbus.h"
#include "jkutil/bitdataop.h"
#include "log/zuclog.h"

#define AXIS_DEBUG(fmt, ...) zuclog_debug("CAN_SERVO%d", fmt, cfg_.devid, ##__VA_ARGS__)
#define AXIS_INFO(fmt, ...) zuclog_info("CAN_SERVO%d", fmt, cfg_.devid, ##__VA_ARGS__)

using rtdev::JkServo;

enum StateWord
{
    NO_READY_SWON = 0x00,
    SWON_DIS,
    READY_SWON,
    SWON,
    OP_EN,
    QUICK_STOP_ACTIVE,
    FAULT_REACTION_ACTIVE,
    FAULT
};

static StateWord get_stateword(uint16_t sword)
{
    if ((sword & 0x004f) == 0x00)
    {
        return NO_READY_SWON;
    }
    else if ((sword & 0x004f) == 0x40)
    {
        return SWON_DIS;
    }
    else if ((sword & 0x006f) == 0x21)
    {
        return READY_SWON;
    }
    else if ((sword & 0x006f) == 0x23)
    {
        return SWON;
    }
    else if ((sword & 0x006f) == 0x27)
    {
        return OP_EN;
    }
    else if ((sword & 0x006f) == 0x07)
    {
        return QUICK_STOP_ACTIVE;
    }
    else if ((sword & 0x004f) == 0x0f)
    {
        return FAULT_REACTION_ACTIVE;
    }
    else if ((sword & 0x004f) == 0x08)
    {
        return FAULT;
    }
    return NO_READY_SWON;
}

static const char* get_stateword_name(uint16_t sword)
{
    switch (get_stateword(sword))
    {
    case NO_READY_SWON:
        return "no_ready_swon";
    case SWON_DIS:
        return "swon_dis";
    case READY_SWON:
        return "ready_swon";
    case SWON:
        return "swon";
    case OP_EN:
        return "op_en";
    case QUICK_STOP_ACTIVE:
        return "quick_stop_active";
    case FAULT_REACTION_ACTIVE:
        return "fault_reaction_active";
    case FAULT:
        return "fault";
    default:
        return "unknown";
    }
}

static constexpr uint32_t ota_feedback_id(uint8_t id) { return 0x00C00000 + 0x20 * (id + 1); }
static constexpr uint32_t feedback_id(uint8_t id) { return 0x00A00000 + 0x20 * (id + 1); }
static constexpr uint32_t error_id() { return 0x003FFFE0; }
static constexpr uint32_t cmd_id(uint8_t id) { return 0x00600000 + 0x2000 * (id + 1); }

JkServo::JkServo() {}

int JkServo::boot()
{
    if (!bus_.get())
    {
        return -1;
    }
    bus_->config_rxpdo(ota_feedback_id(cfg_.devid), 8);  //ota feedback
    bus_->config_rxpdo(error_id(), 8);                   //error
    bus_->config_rxpdo(feedback_id(cfg_.devid), 8);      //feedback
    return 0;
}
int JkServo::shutdown() { return -1; }
int JkServo::set_opt(std::string opt_name, void* data, size_t s)
{
    if (opt_name == "torq-enable")
    {
        is_dyn_forward_ = *(uint8_t*)data;
    }
    else if (opt_name == "collision-level")
    {
        cmd_collision_lv_ = *(uint8_t*)data;
    }
    return 0;
}

int JkServo::set_send_data(int busid)
{
    if (is_sim())
        return 1;
    if (!bus_.get())
        return -1;
    last_alive_ = is_alive();
    check_alive();

    auto tmp_cword = handle_control_word(stat_.statusword, 1);
    if (cmd_.controlword != tmp_cword)
    {
        AXIS_DEBUG("cword 0x%x->0x%x", cmd_.controlword, tmp_cword);
    }
    cmd_.controlword = tmp_cword;

    // pack the tx data
    uint8_t data[8] = {0};
    data[0] = cmd_.controlword >> 8 & 0xff;
    data[1] = cmd_.controlword & 0xff;

    data[2] = cmd_.opmode & 0x0f;
    data[2] |= (cmd_collision_lv_ & 0x07) << 4;
    data[2] |= (is_dyn_forward_ & 0x01) << 7;

    int32_t pos = cmd_.target_pos;
    if (pos < 0)
    {
        data[3] = (abs(pos) >> 16) | 0x80;
    }
    else
    {
        data[3] = (abs(pos) >> 16) & 0xff;
    }

    data[4] = (abs(pos) >> 8) & 0xff;
    data[5] = abs(pos) & 0xff;

    int16_t torq = cmd_.target_torq;
    if (torq < 0)
    {
        data[6] = (abs(torq) >> 8) | 0x80;
    }
    else
    {
        data[6] = (abs(torq) >> 8) & 0xff;
    }
    data[7] = abs(torq) & 0xff;

    // set the txpdo
    bus_->set_txpdo_data(cmd_id(cfg_.devid), data);

    return 0;
}
int JkServo::get_recv_data(int busid)
{
    if (is_sim())
        return 1;
    if (!bus_.get())
        return -1;
    uint8_t data[8] = {0};
    auto alive = bus_->get_rxpdo_data(feedback_id(cfg_.devid), data);
    keep_alive(alive);
    if (is_alive() && !last_alive_)  //从不存活到存活,启动txpdo和sdo
    {
        bus_->config_txpdo(cmd_id(cfg_.devid), 8);  //cmd
        bus_->register_sdo_handle(cfg_.devid + 1, 0x00DFFFE0, 0x00FFFFE0, 0x013FFFE0, 2);
    }

    int16_t status_word = data[0] << 8 | data[1];
    if (status_word != stat_.statusword)
    {
        AXIS_DEBUG("status %s(0x%x)->%s(0x%x)", get_stateword_name(stat_.statusword), stat_.statusword, get_stateword_name(status_word), status_word);
        stat_.statusword = status_word;
    }

    stat_.opmode_display = data[2] & 0x0f;
    stat_.act_collision_lv = data[2] >> 4 & 0x07;
    stat_.act_pos = (data[5] + (data[4] << 8) + ((data[3] & 0x7f) << 16)) * ((data[3] & 0x80) ? -1 : 1);
    stat_.act_torq = (data[7] + ((data[6] & 0x7f) << 8)) * ((data[6] & 0x80) ? -1 : 1);

    // 接收ota data
    uint8_t ota[8] = {0};
    bus_->get_rxpdo_data(ota_feedback_id(cfg_.devid), ota);
    stat_.ota_pos_diff = ota[1] + (ota[0] << 8);
    stat_.ota_dual_encoder_diff = ota[3] + (ota[2] << 8);
    stat_.ota_dc_vol = ota[5] + (ota[4] << 8);

    // 接收error
    uint8_t edata[2] = {0};
    bus_->get_rxpdo_data(error_id(), edata);
    if (edata[0] == cfg_.devid + 1)
    {
        stat_.errorcode = edata[2] | edata[3] << 8 | edata[4] << 16;
    }
    return 0;
}
int JkServo::get_sdo_blocking(uint32_t objid, void* data, int s, int retry)
{
    if (is_sim())
        return 0;
    uint16_t id = (objid / 100 << 8) | (objid % 100 & 0xff);
    int err = bus_->get_sdo_blocking(cfg_.devid + 1, id, data, s, retry);
    return err;
}
int JkServo::set_sdo_blocking(uint32_t objid, void* data, size_t s, void* readback, int retry)
{
    if (is_sim())
        return 0;
    uint16_t id = (objid / 100 << 8) | (objid % 100 & 0xff);
    int err = bus_->set_sdo_blocking(cfg_.devid + 1, id, data, s, readback, retry);
    return err;
}

uint16_t JkServo::handle_control_word(uint16_t sword, bool has_ext_enable_bit)
{
    uint16_t cword = 0;
    uint8_t* cword_ptr = (uint8_t*)&cword;

    auto cur_state = get_stateword(sword);
    bool iserror = false;
    bool stat_enable = false;
    switch (cur_state)
    {
    case NO_READY_SWON:
        cword = 0x00;
        break;
    case SWON_DIS:
        cword = 0x06;  //shutdown
        break;
    case READY_SWON:
        cword = this->cmd_enable_ ? 0x07 : 0x06;
        break;
    case SWON:
        cword = this->cmd_enable_ ? 0x0f : 0x06;
        break;
    case OP_EN: {
        stat_enable = true;
        cword = this->cmd_enable_ ? 0x0f : 0x06;
        //在支持扩展使能位的情况下，需要额外判定
        if (has_ext_enable_bit && (sword >> 14 & 0x01) == 0)
        {
            stat_enable = false;
        }

        break;
    }
    case QUICK_STOP_ACTIVE:
        cword = 0x06;  //shutdown
        break;
    case FAULT_REACTION_ACTIVE:
        iserror = true;
        break;

    case FAULT: {
        iserror = true;
        cword = this->cmd_.controlword;
        if (this->cmd_.error_reset)
        {
            Util::set_bit(cword_ptr, 7);
            this->cmd_.error_reset = false;
        }
        else
        {
            Util::clr_bit(cword_ptr, 7);
        }
        break;
    }
    default:
        break;
    }
    stat_.error_sta = iserror ? 1 : 0;

    //碰撞状态处理
    stat_.act_collision_lv = sword >> 15 & 0x01;
    if (stat_.act_collision_lv)
    {
        if (cmd_.collision_reset > 0)
        {
            Util::set_bit(cword_ptr, 11);  //cword |= (1 << 11);
            cmd_.collision_reset--;
        }
        else
        {
            Util::clr_bit(cword_ptr, 11);
        }
    }

    if (stat_enable != stat_.enable)
    {
        AXIS_DEBUG("enable status change %d->%d", stat_.enable, stat_enable);
        stat_.enable = stat_enable;
    }

    //回零状态处理
    if (stat_.opmode_display == (int)TRACKING_MODE::OPMODE_HOME)
    {
        if (cmd_.home)
        {
            cword |= 0x10;
            cmd_.home = 0;
        }
        if ((sword & 0x3400) == 0x1400)  //home reached
        {
            Util::clr_bit(cword_ptr, 12);
            stat_.home = 1;
        }
    }
    return cword;
}

bool rtdev::JkServo::is_enable()
{
    if (is_sim())
        return cmd_enable_;
    return stat_.enable;
}
bool rtdev::JkServo::is_homed() { return stat_.home; }
uint32_t rtdev::JkServo::get_act_collision_lv() { return 0; }
bool rtdev::JkServo::is_tor_overload() { return false; }
bool rtdev::JkServo::is_collision() { return false; }
bool rtdev::JkServo::is_backdrive() { return false; }
